////////////////////////////////////////////////////////////
// Headers
////////////////////////////////////////////////////////////
#include "QuadTree.hpp"

#include <assert.h>

#include "../BoundingVolumes/BoundingBox.hpp"

#include "QuadTreeNode.hpp"

#include <SmegTools/Debug/DebugNew.hpp>

namespace smeg {
    namespace math {
        
////////////////////////////////////////////////////////////
/// Default Constructor
////////////////////////////////////////////////////////////
QuadTree::QuadTree() :
    m_pRoot( NULL ),
    m_OutsideOccupants()
{
}
    
////////////////////////////////////////////////////////////
/// Constructor
////////////////////////////////////////////////////////////
QuadTree::QuadTree( const math::BoundingBox& _Region ) :
    m_pRoot( NULL ),
    m_OutsideOccupants()
{
    Create( _Region );
}
    
////////////////////////////////////////////////////////////
/// Destructor
////////////////////////////////////////////////////////////
QuadTree::~QuadTree()
{
    Clear();
}

////////////////////////////////////////////////////////////
/// Create the tree
////////////////////////////////////////////////////////////
void QuadTree::Create( const math::BoundingBox& _Region ) {
    std::vector< core::BaseObject* > AllOccupants;

    if ( m_pRoot ) {
        m_pRoot->GetAllOccupantsBelow( AllOccupants );

        for ( td_OccupantsSetIt it = m_OutsideOccupants.begin(); 
            it != m_OutsideOccupants.end();
            ++it ) {
            AllOccupants.push_back( *it );
        }
    }

    Clear();

    m_pRoot = new QuadTreeNode( _Region, this );

    for ( unsigned i = 0; i < AllOccupants.size(); ++i ) {
        Add( AllOccupants[i] );
    } 
}

////////////////////////////////////////////////////////////
/// Clear the tree
////////////////////////////////////////////////////////////
void QuadTree::Clear() {
    delete m_pRoot;
    m_pRoot = NULL;

    m_OutsideOccupants.clear();
}

////////////////////////////////////////////////////////////
/// Add an occupant to the tree
////////////////////////////////////////////////////////////
void QuadTree::Add( core::BaseObject* _pOccupant ) {
    assert( m_pRoot );

    if ( !m_pRoot->Add( _pOccupant ) )
        AddOutside( _pOccupant );
}

////////////////////////////////////////////////////////////
/// Add an occupant outside the tree
////////////////////////////////////////////////////////////
void QuadTree::AddOutside( core::BaseObject* _pOccupant ) {
    assert( m_pRoot );
    
    _pOccupant->SetTree( this );
    _pOccupant->SetNode( NULL );
    m_OutsideOccupants.insert( _pOccupant );
}

////////////////////////////////////////////////////////////
/// Update the occupation of this tree with the given occupant
////////////////////////////////////////////////////////////
void QuadTree::Update( core::BaseObject* _pOccupant ) {
    QuadTreeNode* pOccupantNode = _pOccupant->GetNode();
    if ( pOccupantNode ) {
        pOccupantNode->Update( _pOccupant );
    } else {
        m_OutsideOccupants.erase( _pOccupant );

        Add( _pOccupant );
    }
}

////////////////////////////////////////////////////////////
/// Remove an occupant from this tree
////////////////////////////////////////////////////////////
void QuadTree::Remove( core::BaseObject* _pOccupant ) {
    QuadTreeNode* pOccupantNode = _pOccupant->GetNode();
    if ( pOccupantNode ) {
        pOccupantNode->Remove( _pOccupant );
    } else {
        m_OutsideOccupants.erase( _pOccupant );
    }
}

////////////////////////////////////////////////////////////
/// Query a region and retrieve the occupants in this region
////////////////////////////////////////////////////////////
void QuadTree::QueryRegion( const math::BoundingBox& _Region, std::vector<core::BaseObject*>& _Result, BoundingVolume::eBoundingVolume _eType ) const {
    assert( m_pRoot );

    core::BaseObject* pCurrentOccupant;

    // Query outside occupants
    for ( td_OccupantsSetIt it = m_OutsideOccupants.begin(); it != m_OutsideOccupants.end(); ++it ) {
        pCurrentOccupant = *it;
                
        if ( pCurrentOccupant->HasBoundingVolume( _eType ) &&
            ( _Region.Contains( pCurrentOccupant->getPosition() ) ||
            _Region.OverlapsUnknownBBPtr( pCurrentOccupant->GetBoundingVolume() ) ) ) {
            _Result.push_back( pCurrentOccupant );
        }
    }

    // Query node
    QuadTreeNode* pCurrentNode;
    std::vector<QuadTreeNode*> nodeToParse;

    nodeToParse.push_back( m_pRoot );

    while ( !nodeToParse.empty() ) {
        pCurrentNode = nodeToParse.back();
        nodeToParse.pop_back();

        if ( _Region.Contains( pCurrentNode->GetRegion() ) ) {
            // Add all occupants without checking
            pCurrentNode->GetAllOccupantsBelow( _Result, _eType );
        } else if ( _Region.Overlaps( pCurrentNode->GetRegion() ) ) {
            // Check occupants of the node
            const td_OccupantsSet& occupants( pCurrentNode->GetOccupants() );
            for ( td_OccupantsSetConstIt it = occupants.begin(); it != occupants.end(); ++it ) {
                pCurrentOccupant = *it;
                
                if ( pCurrentOccupant->HasBoundingVolume( _eType ) &&
                    ( _Region.Contains( pCurrentOccupant->getPosition() ) ||
                    _Region.OverlapsUnknownBBPtr( pCurrentOccupant->GetBoundingVolume() ) ) ) {
                    _Result.push_back( pCurrentOccupant );
                }
            }

            // Get all children node if any
            if ( pCurrentNode->HasChildren() ) {
                for ( unsigned x = 0; x < 2; ++x ) {
                    for ( unsigned y = 0; y < 2; ++y ) {
                        nodeToParse.push_back( pCurrentNode->GetChild( x, y ) );
                    }
                }
            }
        }
    }
}

#ifdef _DEBUG
void QuadTree::DebugRender( sf::RenderWindow* _pRenderWindow ) const {
    if ( m_pRoot ) {
        m_pRoot->DebugRender( _pRenderWindow );
    }
}
#endif // _DEBUG

    } // namespace light
} // namespace smeg